﻿using System;
using System.Collections.Generic;
using System.Configuration;
using System.Diagnostics;
using System.Drawing;
using System.IO;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using OpenTK;
using OpenTK.Graphics;

namespace LasRendering
{
    public class VertexBucket
    {
        public VertexClassification Classification;
        public IntPtr Ptr;
        public int CountInMemory;
        public int CountInVBO;
        public int BytesPerVertex;
        public int VertexBufferObjectId;
        public float LoadingRatio;

        public const int GrndBucketId = 0;
        public const int TreeBucketId = 3;
        public const int RoofBucketId = 2;
        public const int WallBucketId = 1;
        public const int NumBuckets = 4;
    }

    public static class VertexHelper
    {
        [StructLayout(LayoutKind.Sequential)]
        private struct VertexStruct
        {
            public Color4 color;
            public Vector3 normal;
            public Vector3 position;
        }

        private static Color4 grndColor;
        private static Color4 treeColor;
        private static Color4 roofColor;
        private static Color4 wallColor;
        private static int numNeighbors;
        private static float numNeighborsSqrt;
        private static WallDrawingMethod wallDrawingMethod;

        private static string filename;
        private static string lazFilename;
        private static bool isLasFile;

        private static float neighborsRangeFactor;
        private static float minAngleForRoofEdgeVertex;
        private static float sleeveSizeFactor;
        private static float maxAngleDeltaForWallMerge;
        private static float maxDistanceFactorForWallMerge;
        private static int minVerticesOfAnEdge;

        public static void Init()
        {
            Vector3 color;
            color = VectorHelper.Parse(ConfigurationManager.AppSettings["groundColor"]);
            grndColor = new Color4(color.X, color.Y, color.Z, 1);
            color = VectorHelper.Parse(ConfigurationManager.AppSettings["treeColor"]);
            treeColor = new Color4(color.X, color.Y, color.Z, 1);
            color = VectorHelper.Parse(ConfigurationManager.AppSettings["roofColor"]);
            roofColor = new Color4(color.X, color.Y, color.Z, 1);
            color = VectorHelper.Parse(ConfigurationManager.AppSettings["wallColor"]);
            wallColor = new Color4(color.X, color.Y, color.Z, 1); //alpha will be ignored later (replaced with groundLevel)
            numNeighbors = Convert.ToInt32(ConfigurationManager.AppSettings["numNeighborsForComputation"]);
            numNeighborsSqrt = (float)Math.Sqrt(numNeighbors);
            wallDrawingMethod = (WallDrawingMethod)Enum.Parse(typeof(WallDrawingMethod), ConfigurationManager.AppSettings["wallDrawingMethod"], true);

            neighborsRangeFactor = Convert.ToSingle(ConfigurationManager.AppSettings["neighborsRangeFactor"]);

            minAngleForRoofEdgeVertex = MathHelper.DegreesToRadians(float.Parse(ConfigurationManager.AppSettings["minAngleForRoofEdgeVertex"]));
            if (wallDrawingMethod == WallDrawingMethod.Plane)
            {
                sleeveSizeFactor = float.Parse(ConfigurationManager.AppSettings["sleeveSizeFactor"]);
                maxAngleDeltaForWallMerge = MathHelper.DegreesToRadians(float.Parse(ConfigurationManager.AppSettings["maxAngleDeltaForWallMerge"]));
                maxDistanceFactorForWallMerge = float.Parse(ConfigurationManager.AppSettings["maxDistanceFactorForWallMerge"]);
                minVerticesOfAnEdge = int.Parse(ConfigurationManager.AppSettings["minVerticesOfAnEdge"]);
            }
        }

        #region Preprocessing

        public static QuadTree Preprocess()
        {
            filename = ConfigurationManager.AppSettings["filename"];
            var extension = Path.GetExtension(filename).ToLower();
            Debug.Assert(extension == ".las" || extension == ".laz");
            isLasFile = extension == ".las";
            lazFilename = Path.ChangeExtension(filename, ".laz");

            Log.Write("Filename: {0}", filename);

            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();

            var quadTree = LasHelper.CreateQuadTree(filename);
            if (quadTree.VertexAvgSize == 0)
            {
                quadTree.VertexAvgSize = (float)Math.Sqrt((quadTree.MaxPosition.X - quadTree.MinPosition.X) * (quadTree.MaxPosition.Y - quadTree.MinPosition.Y) / quadTree.Count * Math.Sqrt(2));
            }

            stopwatch.Stop();
            Log.Write("Load and create QuadTree: {0} ms", stopwatch.ElapsedMilliseconds);

            stopwatch.Reset();
            stopwatch.Start();

            if (isLasFile)
            {
                if (VertexColorHelper.Init(quadTree.MinPosition))
                {
                    grndColor = new Color4(0f, 0f, 0f, 0f);
                    treeColor = new Color4(0f, 0f, 0f, 0f);
                    roofColor = new Color4(0f, 0f, 0f, 0f);
                    wallColor = new Color4(0f, 0f, 0f, 0f);
                }
                LasHelper.InitSavingFile(filename);
            }

            int index = 0;
            while (index < quadTree.LeafNodes.Count)
            {
                var events = new ManualResetEvent[Math.Min(quadTree.LeafNodes.Count - index, 64)];
                for (int i = 0; index < quadTree.LeafNodes.Count && i < 64; index++, i++)
                {
                    var node = quadTree.LeafNodes[index];
                    var @event = new ManualResetEvent(false);
                    events[i] = @event;
#if DEBUG
                    PreprocessMain(new object[] { node, quadTree.MinPosition, quadTree.MaxPosition, quadTree.VertexAvgSize, @event });
#else
                    ThreadPool.QueueUserWorkItem(PreprocessMain, new object[] { node, quadTree.MinPosition, quadTree.MaxPosition, quadTree.VertexAvgSize, @event });
#endif
                }
                WaitHandle.WaitAll(events);
                for (int i = 0; i < events.Length; i++)
                {
                    events[i].Close();
                }
            }

            int count = 0;
            foreach (var node in quadTree.LeafNodes)
            {
                count += node.VertexIndexLink.Count;
            }
            quadTree.Count = count;

            quadTree.OffsetBoundary(quadTree.MinPosition.Xy);
            if (isLasFile)
            {
                quadTree.MaxPosition -= quadTree.MinPosition;
                quadTree.MinPosition = new Vector3(0, 0, 0);
                LasHelper.FinishSavingFile(quadTree.MinPosition, quadTree.MaxPosition, quadTree.VertexAvgSize);
            }

            // Swap Y and Z. In LAS, Z is height, in GPU World model, Y is height.
            var minPosition = new Vector3(quadTree.MinPosition.X, quadTree.MinPosition.Z, -quadTree.MaxPosition.Y);
            var maxPosition = new Vector3(quadTree.MaxPosition.X, quadTree.MaxPosition.Z, -quadTree.MinPosition.Y);
            quadTree.MinPosition = minPosition;
            quadTree.MaxPosition = maxPosition;

            stopwatch.Stop();
            Log.Write("Compute normal and color: {0} ms", stopwatch.ElapsedMilliseconds);

            stopwatch.Reset();
            stopwatch.Start();

            if (isLasFile)
            {
                VertexColorHelper.PreprocessFindColor(filename, quadTree);
            }

            stopwatch.Stop();
            Log.Write("Find color: {0} ms", stopwatch.ElapsedMilliseconds);

            return quadTree;
        }

        private static void PreprocessMain(object state)
        {
            object[] args = (object[])state;
            var node = (QuadTreeNode)args[0];
            var minPosition = (Vector3)args[1];
            var maxPosition = (Vector3)args[2];
            var vertexAvgSize = (float)args[3];
            var @event = (ManualResetEvent)args[4];

            List<Vertex> grndVertexList;
            List<Vertex> roofVertexList;
            List<Vertex> treeVertexList;
            List<Vertex> wallVertexList;
            List<Vertex> extraGrndVertexList;
            List<Vertex> neighborGrndVertexList;
            List<Vertex> neighborRoofVertexList;
            List<Vertex> neighborTreeVertexList;
            List<Vertex> neighborWallVertexList;
            List<Vertex> neighborExtraGrndVertexList;
            int discardedVertexCount;

            PreprocessLoadVertices(node.VertexIndexLink, out grndVertexList, out roofVertexList, out treeVertexList, out wallVertexList, out extraGrndVertexList, out discardedVertexCount);
            PreprocessLoadVertices(node.NeighborVertexIndexLink, out neighborGrndVertexList, out neighborRoofVertexList, out neighborTreeVertexList, out neighborWallVertexList, out neighborExtraGrndVertexList, out discardedVertexCount);

            if (isLasFile)
            {
                // Minus minPosition for every vertex to minimize float tolerances
                PreprocessOffsetVertices(grndVertexList, minPosition);
                PreprocessOffsetVertices(roofVertexList, minPosition);
                PreprocessOffsetVertices(treeVertexList, minPosition);
                PreprocessOffsetVertices(wallVertexList, minPosition);
                PreprocessOffsetVertices(extraGrndVertexList, minPosition);
                PreprocessOffsetVertices(neighborGrndVertexList, minPosition);
                PreprocessOffsetVertices(neighborRoofVertexList, minPosition);
                PreprocessOffsetVertices(neighborTreeVertexList, minPosition);
                PreprocessOffsetVertices(neighborWallVertexList, minPosition);
                PreprocessOffsetVertices(neighborExtraGrndVertexList, minPosition);

                PreprocessComputation(
                    vertexAvgSize,
                    grndVertexList,
                    roofVertexList,
                    treeVertexList,
                    wallVertexList,
                    extraGrndVertexList,
                    neighborGrndVertexList,
                    neighborRoofVertexList,
                    neighborTreeVertexList);

                if (wallDrawingMethod == WallDrawingMethod.Points || wallDrawingMethod == WallDrawingMethod.Plane)
                {
                    PreprocessSaveVertices(node, grndVertexList, roofVertexList, treeVertexList, extraGrndVertexList, wallVertexList);
                }
                else
                {
                    PreprocessSaveVertices(node, grndVertexList, roofVertexList, treeVertexList, extraGrndVertexList);
                }
            }

            @event.Set();
        }

        private static void PreprocessOffsetVertices(List<Vertex> vertexList, Vector3 offset)
        {
            foreach (var vertex in vertexList)
            {
                vertex.Position -= offset;
            }
        }

        private static void PreprocessComputation(
            float vertexAvgSize,
            List<Vertex> grndVertexList,
            List<Vertex> roofVertexList,
            List<Vertex> treeVertexList,
            List<Vertex> wallVertexList,
            List<Vertex> extraGrndVertexList,
            List<Vertex> neighborGrndVertexList,
            List<Vertex> neighborRoofVertexList,
            List<Vertex> neighborTreeVertexList)
        {
            // Set dimensionality to 2, build kd-tree on XY plane only.
            var grndKdTree = new KdTree(grndVertexList, 2);
            grndKdTree.Add(neighborGrndVertexList);
            var roofKdTree = new KdTree(roofVertexList, 2);
            roofKdTree.Add(neighborRoofVertexList);
            var treeKdTree = new KdTree(treeVertexList, 2);
            treeKdTree.Add(neighborTreeVertexList);

            ComputeOnTreeVertex(treeVertexList, grndKdTree, treeKdTree, vertexAvgSize, extraGrndVertexList);
            grndKdTree.Add(extraGrndVertexList);
            ComputeOnGrndVertex(grndVertexList, grndKdTree, vertexAvgSize);
            ComputeOnGrndVertex(extraGrndVertexList, grndKdTree, vertexAvgSize);
            ComputeOnRoofVertex(roofVertexList, grndKdTree, roofKdTree, vertexAvgSize, wallVertexList);

            var wallKdTree = new KdTree(wallVertexList, 3);
            ComputeOnWallVertex(wallVertexList, wallKdTree, grndKdTree, vertexAvgSize);
        }

        private static void PreprocessLoadVertices(
            QuadTreeVertexIndexLink vertexIndexLink,
            out List<Vertex> grndVertexList,
            out List<Vertex> roofVertexList,
            out List<Vertex> treeVertexList,
            out List<Vertex> wallVertexList,
            out List<Vertex> extraGrndVertexList,
            out int discardedVertexCount)
        {
            List<Vertex> vertexList = LasHelper.ReadVertices(filename, vertexIndexLink, int.MaxValue);

            discardedVertexCount = 0;
            grndVertexList = new List<Vertex>(vertexList.Count / 4);
            roofVertexList = new List<Vertex>(vertexList.Count / 4);
            treeVertexList = new List<Vertex>(vertexList.Count / 4);
            wallVertexList = new List<Vertex>(vertexList.Count / 4);
            extraGrndVertexList = new List<Vertex>();
            foreach (var vertex in vertexList)
            {
                switch (vertex.Classification)
                {
                    case VertexClassification.Ground:
                        if (vertex.Color.A == 0)
                        {
                            vertex.Color = grndColor;
                        }
                        grndVertexList.Add(vertex);
                        break;
                    case VertexClassification.Roof:
                        if (vertex.Color.A == 0)
                        {
                            vertex.Color = roofColor;
                        }
                        roofVertexList.Add(vertex);
                        break;
                    case VertexClassification.Tree:
                        if (vertex.Color.A == 0)
                        {
                            vertex.Color = treeColor;
                        }
                        treeVertexList.Add(vertex);
                        break;
                    case VertexClassification.Wall:
                        if (vertex.Color.A == 0)
                        {
                            vertex.Color = wallColor;
                        }
                        wallVertexList.Add(vertex);
                        break;
                    case VertexClassification.Roof | VertexClassification.Wall:
                        if (vertex.Color.A == 0)
                        {
                            vertex.Color = roofColor;
                        }
                        roofVertexList.Add(vertex);
                        wallVertexList.Add(vertex);
                        break;
                    case VertexClassification.Discarded:
                    default:
                        discardedVertexCount++;
                        break;
                }
            }
        }

        private static object savingLock = new object();
        private static void PreprocessSaveVertices(QuadTreeNode node, params List<Vertex>[] vertexLists)
        {
            lock (savingLock)
            {
                int startIndex;
                int count;
                LasHelper.SaveVertices(vertexLists, out startIndex, out count);
                node.NeighborVertexIndexLink.Clear();
                node.VertexIndexLink.Clear();
                node.VertexIndexLink.Add(startIndex, count);
            }
        }

        #endregion

        #region Rendering phase

        public static VertexBucket[] CreateVertexBuckets(QuadTree quadTree, QuadTreeNode node, int expectedNumOfVertices)
        {
            List<Vertex> vertexList = LasHelper.ReadVertices(lazFilename, node.VertexIndexLink, expectedNumOfVertices);

            List<Vertex> grndVertexList = new List<Vertex>(vertexList.Count / 4);
            List<Vertex> roofVertexList = new List<Vertex>(vertexList.Count / 4);
            List<Vertex> treeVertexList = new List<Vertex>(vertexList.Count / 4);
            List<Vertex> wallVertexList = new List<Vertex>(vertexList.Count / 4);
            foreach (var vertex in vertexList)
            {
                switch (vertex.Classification)
                {
                    case VertexClassification.Ground:
                        grndVertexList.Add(vertex);
                        break;
                    case VertexClassification.Roof:
                        roofVertexList.Add(vertex);
                        break;
                    case VertexClassification.Tree:
                        treeVertexList.Add(vertex);
                        break;
                    case VertexClassification.Wall:
                        wallVertexList.Add(vertex);
                        break;
                    case VertexClassification.Roof | VertexClassification.Wall:
                        roofVertexList.Add(vertex);
                        wallVertexList.Add(vertex);
                        break;
                    case VertexClassification.Discarded:
                    default:
                        break;
                }
            }

            var vertexBuckets = new VertexBucket[VertexBucket.NumBuckets];
            vertexBuckets[VertexBucket.GrndBucketId] = new VertexBucket();
            vertexBuckets[VertexBucket.GrndBucketId].Classification = VertexClassification.Ground;
            vertexBuckets[VertexBucket.TreeBucketId] = new VertexBucket();
            vertexBuckets[VertexBucket.TreeBucketId].Classification = VertexClassification.Tree;
            vertexBuckets[VertexBucket.RoofBucketId] = new VertexBucket();
            vertexBuckets[VertexBucket.RoofBucketId].Classification = VertexClassification.Roof;
            vertexBuckets[VertexBucket.WallBucketId] = new VertexBucket();
            vertexBuckets[VertexBucket.WallBucketId].Classification = VertexClassification.Wall;

            LoadVertexBucket(vertexBuckets[VertexBucket.GrndBucketId], grndVertexList, true);
            LoadVertexBucket(vertexBuckets[VertexBucket.TreeBucketId], treeVertexList, true);
            LoadVertexBucket(vertexBuckets[VertexBucket.RoofBucketId], roofVertexList, true);
            LoadVertexBucket(vertexBuckets[VertexBucket.WallBucketId], wallVertexList, true);

            float ratio = 1;
            if (expectedNumOfVertices != int.MaxValue)
            {
                float actualCount = 0;
                actualCount += vertexBuckets[VertexBucket.GrndBucketId].CountInMemory;
                actualCount += vertexBuckets[VertexBucket.TreeBucketId].CountInMemory;
                actualCount += vertexBuckets[VertexBucket.RoofBucketId].CountInMemory;
                actualCount += vertexBuckets[VertexBucket.WallBucketId].CountInMemory;
                float maxCount = quadTree.Count / quadTree.LeafNodes.Count;
                ratio = actualCount * maxCount / node.VertexIndexLink.Count / expectedNumOfVertices;
                vertexBuckets[VertexBucket.GrndBucketId].LoadingRatio = ratio;
                vertexBuckets[VertexBucket.TreeBucketId].LoadingRatio = ratio;
                vertexBuckets[VertexBucket.RoofBucketId].LoadingRatio = ratio;
                vertexBuckets[VertexBucket.WallBucketId].LoadingRatio = ratio;
            }

            return vertexBuckets;
        }

        public static void CreateVertexIndexBucket(QuadTreeNode node, out VertexBucket vertexBucket, out Color4[] colorList)
        {
            List<Vertex> vertexList = LasHelper.ReadVertices(lazFilename, node.VertexIndexLink, int.MaxValue);

            vertexBucket = new VertexBucket();
            vertexBucket.Classification = VertexClassification.Index;
            LoadVertexBucket(vertexBucket, vertexList, false);

            colorList = new Color4[vertexList.Count];
            for (int i = 0; i < vertexList.Count; i++)
            {
                colorList[i] = vertexList[i].Color;
            }
        }

        public static void FreeMemory(VertexBucket vertexBucket)
        {
            if (vertexBucket.Ptr == IntPtr.Zero)
            {
                return;
            }

            Marshal.FreeHGlobal(vertexBucket.Ptr);
            vertexBucket.Ptr = IntPtr.Zero;
            vertexBucket.CountInMemory = 0;
        }

        private static void LoadVertexBucket(VertexBucket vertexBucket, List<Vertex> vertexList, bool allowDiscardVertices)
        {
            vertexBucket.CountInMemory = vertexList.Count;
            vertexBucket.BytesPerVertex = Marshal.SizeOf(typeof(VertexStruct));
            vertexBucket.Ptr = vertexBucket.CountInMemory == 0 ? IntPtr.Zero : Marshal.AllocHGlobal(vertexBucket.BytesPerVertex * vertexBucket.CountInMemory);

            IntPtr ptr = vertexBucket.Ptr;
            for (int i = 0; i < vertexList.Count; i++)
            {
                Vertex vertex = vertexList[i];

                if (allowDiscardVertices && (vertex.Classification == VertexClassification.Discarded || vertex.Color.A == 0))
                {
                    vertexBucket.CountInMemory--;
                    continue;
                }

                VertexStruct vertexBuf;

                if (vertexBucket.Classification == VertexClassification.Index)
                {
                    var color = Color.FromArgb(vertex.Index);
                    //if alpha is 0, the color will be treated as 00000000 in GPU, so we make it not 0.
                    vertexBuf.color = new Color4(color.R, color.G, color.B, (byte)(byte.MaxValue - color.A));
                }
                else
                {
                    vertexBuf.color = vertex.Color;
                }

                //Swap Y and Z.
                vertexBuf.normal.X = vertex.Normal.X;
                vertexBuf.normal.Y = vertex.Normal.Z;
                vertexBuf.normal.Z = -vertex.Normal.Y;
                vertexBuf.position.X = vertex.Position.X;
                vertexBuf.position.Y = vertex.Position.Z;
                vertexBuf.position.Z = -vertex.Position.Y;

                if (vertexBucket.Classification == VertexClassification.Wall && wallDrawingMethod != WallDrawingMethod.Points)
                {
                    //Swap Y and Z.
                    vertexBuf.normal.X = vertex.WallNormal.X;
                    vertexBuf.normal.Y = vertex.WallNormal.Z;
                    vertexBuf.normal.Z = -vertex.WallNormal.Y;
                    vertexBuf.color = vertex.Color;
                    vertexBuf.color.A = vertex.GroundLevel;
                }

                // No need to call DestroyStructure because VertexStruct is a blittable structure.
                Marshal.StructureToPtr(vertexBuf, ptr, false);
                ptr = new IntPtr(ptr.ToInt64() + vertexBucket.BytesPerVertex);
            }
        }

        #endregion

        #region Compute Vertices for Tree/Ground/Roof/Wall

        private static void ComputeOnTreeVertex(List<Vertex> treeVertexList, KdTree grndKdTree, KdTree treeKdTree, float vertexAvgSize, List<Vertex> extraGrndVertexList)
        {
            for (int i = 0; i < treeVertexList.Count; i++)
            {
                var vertex = treeVertexList[i];

                // Restrict range to speed up search.
                var neighbors = treeKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, vertexAvgSize * numNeighborsSqrt);
                if (neighbors.Count < 3)
                {
                    // The point is kind of orphan, cannot find enough neighbor points.
                    // Discard the point.
                    vertex.Classification = VertexClassification.Discarded;
                    continue;
                }
                vertex.Normal = ComputeNormal(neighbors, vertex);

                // Next, generate another ground vertex for every tree vertex.
                var range = neighbors.MaxPriority;
                // Restrict range to speed up search.
                neighbors = grndKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, range);
                if (neighbors.Count == 0)
                {
                    neighbors = grndKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors);
                    if (neighbors.Count == 0)
                    {
                        // Happen when there's no ground vertex.
                        continue;
                    }
                }

                // Compute the average height of ground.
                float sumZ = 0;
                foreach (var neighborVertex in neighbors)
                {
                    sumZ += neighborVertex.Position.Z;
                }
                float groundZ = sumZ / neighbors.Count;

                Vertex existingGrndVertex = neighbors.Pop();
                Vertex grndVertex = new Vertex();
                grndVertex.Classification = VertexClassification.Ground;
                grndVertex.Color = existingGrndVertex.Color;
                grndVertex.Position = new Vector3(vertex.Position.X, vertex.Position.Y, groundZ);
                extraGrndVertexList.Add(grndVertex);
            }
        }

        private static void ComputeOnGrndVertex(List<Vertex> grndVertexList, KdTree grndKdTree, float vertexAvgSize)
        {
            for (int i = 0; i < grndVertexList.Count; i++)
            {
                var vertex = grndVertexList[i];

                // Restrict range to speed up search.
                var neighbors = grndKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, vertexAvgSize * numNeighborsSqrt);
                if (neighbors.Count < 3)
                {
                    // The vertex is orphan, cannot find enough neighbor points.
                    // Don't discard, set normal to default: pointing to sky.
                    vertex.Normal = new Vector3(0, 0, 1);
                    continue;
                }
                vertex.Normal = ComputeNormal(neighbors, vertex);
            }
        }

        private static void ComputeOnRoofVertex(List<Vertex> roofVertexList, KdTree grndKdTree, KdTree roofKdTree, float vertexAvgSize, List<Vertex> wallVertexList)
        {
            for (int i = 0; i < roofVertexList.Count; i++)
            {
                var vertex = roofVertexList[i];

                // Restrict range to speed up search.
                var roofNeighbors = roofKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, vertexAvgSize * numNeighborsSqrt);
                var neighbors = new List<Vertex>(roofNeighbors.Count);
                var maxDeltaZ = vertexAvgSize * 3;
                foreach (var neighborVertex in roofNeighbors)
                {
                    if (Math.Abs(vertex.Position.Z - neighborVertex.Position.Z) < maxDeltaZ
                        && neighborVertex.Position != vertex.Position)
                    {
                        neighbors.Add(neighborVertex);
                    }
                }
                if (neighbors.Count < 3)
                {
                    // The vertex is orphan, cannot find enough neighbor points.
                    // Discard it.
                    vertex.Classification = VertexClassification.Discarded;
                    continue;
                }

                vertex.Normal = ComputeNormal(neighbors, vertex);
                //Show the normal difference between xyz and computed normal
                //vertex.Color = new Color4(vertex.Color.R - vertex.Normal.X, vertex.Color.G - vertex.Normal.Y, vertex.Color.B - vertex.Normal.Z, 1);

                // New method to find roof edge points:
                // Connect the point with its neighbors, and compute angles between these lines, 
                // if an angle is greater than a pre-set value (ex: 90 degrees) it's an edge point (Wall)
                //float maxAngle = ComputeMaxAngleBetweenNeighbors(vertex, neighbors);
                float maxAngle = ComputeMaxAngleBetweenNeighbors(vertex, neighbors);
                if (maxAngle > minAngleForRoofEdgeVertex)
                {
                    // This is an edge point
                    vertex.Flag = false;
                    //TODO remove
                    //vertex.Color = Color.Red;
                    wallVertexList.Add(vertex);
                }



                //// Next, we need to find those points that are close to walls.
                //// We find all vertices around current one, if number of ground vertices is greater than a threhold, we mark it as Wall
                //float range = Math.Max(roofNeighbors.MaxPriority, vertexAvgSize * neighborsRangeFactor);
                //var grndNeighbors = grndKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, range);
                //int roofVertexCount = roofNeighbors.Count;
                //int grndVertexCount = grndNeighbors.Count;
                //float groundLevel = float.MaxValue;
                //foreach (var neighborVertex in roofNeighbors)
                //{
                //    if ((vertex.Position.Z - neighborVertex.Position.Z) > (vertexAvgSize * 3))
                //    {
                //        // A roof point is significant lower than current roof point, we count it as a ground point, may need to draw wall.
                //        grndVertexCount++;
                //        roofVertexCount--;
                //        groundLevel = Math.Min(groundLevel, neighborVertex.Position.Z);
                //    }
                //}
                //// If there're more than 3 ground points near a roof point, we draw wall on current roof point.
                //if (roofVertexCount >= 3 && grndVertexCount >= 3 && grndVertexCount > (roofVertexCount * 0.3f))
                //{
                //    foreach (var neighborVertex in grndNeighbors)
                //    {
                //        groundLevel = Math.Min(groundLevel, neighborVertex.Position.Z);
                //    }
                //    vertex.GroundLevel = groundLevel;

                //    if (wallDrawingMethod == WallDrawingMethod.Cube)
                //    {
                //        vertex.Classification |= VertexClassification.Wall;
                //        vertex.WallNormal = vertex.Normal;
                //        wallVertexList.Add(vertex);
                //    }
                //    if (wallDrawingMethod == WallDrawingMethod.Plane)
                //    {
                //        //TODO
                //    }
                //    else // (wallDrawingMethod == WallDrawingMethod.Polygon or Points)
                //    {
                //        neighbors = new List<Vertex>(roofNeighbors);
                //        neighbors.Add(vertex);
                //        neighbors.AddRange(grndNeighbors);
                //        Vertex grndVertex = new Vertex();
                //        grndVertex.Position = new Vector3(vertex.Position.X, vertex.Position.Y, groundLevel);
                //        neighbors.Add(grndVertex);

                //        // We are using roof points and ground points to compute wall normal, so it should sightly point to sky.
                //        // If not, reverse and make it pointing to sky
                //        var wallNormal = ComputeNormal(neighbors, vertex);

                //        // Discard horizontal direction normal for wall.
                //        var lengthXY = Math.Sqrt(wallNormal.X * wallNormal.X + wallNormal.Y * wallNormal.Y);
                //        vertex.WallNormal.X = (float)(wallNormal.X / lengthXY);
                //        vertex.WallNormal.Y = (float)(wallNormal.Y / lengthXY);
                //        vertex.WallNormal.Z = 0;

                //        if (wallDrawingMethod == WallDrawingMethod.Points)
                //        {
                //            float vertexHeight = vertex.Position.Z - vertexAvgSize;
                //            while (vertexHeight >= groundLevel)
                //            {
                //                Vertex wallVertex = new Vertex();
                //                wallVertex.Classification = VertexClassification.Wall;
                //                wallVertex.Position = new Vector3(vertex.Position.X, vertex.Position.Y, vertexHeight);
                //                wallVertex.Color = wallColor;
                //                wallVertex.Normal = vertex.WallNormal;
                //                wallVertex.GroundLevel = groundLevel;
                //                wallVertexList.Add(wallVertex);

                //                vertexHeight -= vertexAvgSize;
                //            }
                //        }
                //        if (wallDrawingMethod == WallDrawingMethod.Plane)
                //        {
                //            //TODO
                //        }
                //        else // wallDrawingMethod == WallDrawingMethod.Polygon
                //        {
                //            vertex.Classification |= VertexClassification.Wall;
                //            wallVertexList.Add(vertex);
                //        }
                //    }
                //}
            }
        }

        private static void ComputeOnWallVertex(List<Vertex> wallVertexList, KdTree wallKdTree, KdTree grndKdTree, float vertexAvgSize)
        {
            for (int i = 0; i < wallVertexList.Count; i++)
            {
                var vertex = wallVertexList[i];

                var neighbors = wallKdTree.FindNearestNNeighbors(vertex.Position, numNeighbors, vertexAvgSize * numNeighborsSqrt);
                var direction = ComputeWallDirection(neighbors, vertex.Position, vertexAvgSize);
                vertex.WallNormal = direction;
                vertex.Flag = false;
            }

            if (wallDrawingMethod == WallDrawingMethod.Plane)
            {
                float sleeveRadius = sleeveSizeFactor * vertexAvgSize;
                float findRange = sleeveRadius / (float)Math.Sqrt(3) * 2;
                float findStep = findRange;

                Random random = new Random(1);
                List<RoofEdge> globalEdgeList = new List<RoofEdge>();

                for (int i = 0; i < wallVertexList.Count; i++)
                {
                    var vertex = wallVertexList[i];

                    if (vertex.Flag)
                    {
                        continue;
                    }

                    RoofEdge edge = new RoofEdge(vertex.Position, vertex.WallNormal, sleeveRadius, 4);
                    edge.color = wallColor;
                    //edge.color = new Color4((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), 1);
                    globalEdgeList.Add(edge);

                    vertex.Flag = true;
                    FindWallNeighborsByDirection(wallKdTree, edge, vertex, findRange, findStep);
                    FindWallNeighborsByDirection(wallKdTree, edge, vertex, findRange, -findStep);
                }

                MergeEdges(globalEdgeList, vertexAvgSize, wallKdTree);

                wallVertexList.Clear();

                foreach (var edge in globalEdgeList)
                {
                    if (edge.Count >= minVerticesOfAnEdge)
                    {
                        Vector3 endpoint1, endpoint2;
                        edge.GetEndpoints(out endpoint1, out endpoint2);
                        Vector3 normal = Vector3.Normalize(Vector3.Cross((endpoint2 - endpoint1), Vector3.UnitZ));
                        endpoint1 -= edge.Direction * vertexAvgSize;
                        endpoint2 += edge.Direction * vertexAvgSize;
                        AddWallEndpoints(wallVertexList, grndKdTree, edge, endpoint1, endpoint2, normal);

                        //TODO remove
                        //foreach (var item in edge.Positions)
                        //{
                        //    var q = wallKdTree.FindNearestNNeighbors(item, 1);
                        //    if (q.Count > 0)
                        //    {
                        //        q.Pop().Color = edge.color;
                        //    }
                        //}
                    }
                    else
                    {
                        foreach (var position in edge.Positions)
                        {
                            var neighbors = wallKdTree.FindNearestNNeighbors(position, numNeighbors, vertexAvgSize * numNeighborsSqrt);
                            var direction = ComputeWallDirection(neighbors, position, vertexAvgSize);
                            Vector3 normal = Vector3.Cross(direction, Vector3.UnitZ);
                            Vector3 endpoint1 = position - vertexAvgSize * direction;
                            Vector3 endpoint2 = position + vertexAvgSize * direction;
                            AddWallEndpoints(wallVertexList, grndKdTree, edge, endpoint1, endpoint2, normal);
                        }
                    }
                }
            }
            else if (wallDrawingMethod == WallDrawingMethod.Points)
            {
                //TODO
            }
            else
            {
                for (int i = 0; i < wallVertexList.Count; i++)
                {
                    var vertex = wallVertexList[i];
                    vertex.Classification |= VertexClassification.Wall;
                    vertex.Color = wallColor;
                    vertex.WallNormal = Vector3.Cross(vertex.WallNormal, Vector3.UnitZ);
                    vertex.GroundLevel = FindGroundLevel(grndKdTree, vertex.Position);
                }
            }
        }

        private static void AddWallEndpoints(List<Vertex> wallVertexList, KdTree grndKdTree, RoofEdge edge, Vector3 endpoint1, Vector3 endpoint2, Vector3 normal)
        {
            var wallEndpoint = new Vertex();
            wallEndpoint.Classification = VertexClassification.Wall;
            wallEndpoint.Color = edge.color;
            wallEndpoint.GroundLevel = FindGroundLevel(grndKdTree, endpoint1);
            wallEndpoint.Normal = normal;
            wallEndpoint.Position = endpoint1;
            wallEndpoint.WallNormal = -Vector3.UnitX;
            wallVertexList.Add(wallEndpoint);

            wallEndpoint = new Vertex();
            wallEndpoint.Classification = VertexClassification.Wall;
            wallEndpoint.Color = edge.color;
            wallEndpoint.GroundLevel = FindGroundLevel(grndKdTree, endpoint2);
            wallEndpoint.Normal = normal;
            wallEndpoint.Position = endpoint2;
            wallEndpoint.WallNormal = Vector3.UnitX;
            wallVertexList.Add(wallEndpoint);
        }

        private static float FindGroundLevel(KdTree grndKdTree, Vector3 position)
        {
            var grndNeighbors = grndKdTree.FindNearestNNeighbors(position, numNeighbors);
            float groundLevel = grndNeighbors.Count == 0 ? 0 : float.MaxValue;
            foreach (var grndVertex in grndNeighbors)
            {
                groundLevel = Math.Min(groundLevel, grndVertex.Position.Z);
            }
            return groundLevel;
        }

        private static void FindWallNeighborsByDirection(KdTree wallKdTree, RoofEdge edge, Vertex vertex, float findRange, float findStep)
        {
            Vector3 currentPosition = vertex.Position;
            bool continueSearchEdgePoint = true;
            while (continueSearchEdgePoint)
            {
                continueSearchEdgePoint = false;
                var neighbors = wallKdTree.FindInRange(currentPosition, findRange);
                foreach (var neighbor in neighbors)
                {
                    if (!edge.Contains(neighbor.Position) && edge.IsOnRoofEdge(neighbor.Position))
                    {
                        //TODO remove
                        //neighbor.Color = edge.color;
                        edge.Add(neighbor.Position);
                        continueSearchEdgePoint = true;
                    }

                    if (!neighbor.Flag && CalculateAngleDeltaIgnoreDirection(edge.Direction, neighbor.WallNormal) < maxAngleDeltaForWallMerge)
                    {
                        neighbor.Flag = true;
                    }
                }
                currentPosition += findStep * edge.Direction;
            }
        }

        private static float CalculateAngleDeltaIgnoreDirection(Vector3 first, Vector3 second)
        {
            var angle = Vector3.CalculateAngle(first, second);
            if (angle > (Math.PI / 2))
            {
                angle = (float)Math.PI - angle;
            }
            return angle;
        }

        private static void MergeEdges(List<RoofEdge> globalEdgeList, float vertexAvgSize, KdTree wallKdTree)
        {
            float maxDistanceForWallMerge = maxDistanceFactorForWallMerge * vertexAvgSize;

            for (int i = 0; i < globalEdgeList.Count; i++)
            {
                var edge = globalEdgeList[i];
                int j = i + 1;
                while (j < globalEdgeList.Count)
                {
                    var comparingEdge = globalEdgeList[j];
                    if (ShouldMergeEdges(edge, comparingEdge, vertexAvgSize))
                    {
                        edge.Merge(comparingEdge);
                        globalEdgeList.RemoveAt(j);

                        //TODO remove
                        //foreach (var item in edge.Positions)
                        //{
                        //    var q = wallKdTree.FindNearestNNeighbors(item, 1);
                        //    if (q.Count > 0)
                        //    {
                        //        q.Pop().Color = Color.Blue;
                        //    }
                        //}
                    }
                    else
                    {
                        j++;
                    }
                }
            }
        }

        private static bool ShouldMergeEdges(RoofEdge edge1, RoofEdge edge2, float vertexAvgSize)
        {
            float angle = CalculateAngleDeltaIgnoreDirection(edge1.Direction, edge2.Direction);
            if (angle > maxAngleDeltaForWallMerge)
            {
                return false;
            }

            float maxDistanceForWallMerge = maxDistanceFactorForWallMerge * vertexAvgSize;

            Vector3 endpoint11, endpoint12, endpoint21, endpoint22;
            edge1.GetEndpoints(out endpoint11, out endpoint12);
            edge2.GetEndpoints(out endpoint21, out endpoint22);

            var distance = Math.Sqrt(Math.Max(edge1.DirectionFromPointSquared(endpoint21), edge1.DirectionFromPointSquared(endpoint22)));
            if (distance < maxDistanceForWallMerge)
            {
                float min1, max1, min2, max2;
                if (Math.Abs(edge1.Direction.X) > Math.Abs(edge1.Direction.Y))
                {
                    min1 = Math.Min(endpoint11.X, endpoint12.X);
                    max1 = Math.Max(endpoint11.X, endpoint12.X);
                    min2 = Math.Min(endpoint21.X, endpoint22.X);
                    max2 = Math.Max(endpoint21.X, endpoint22.X);
                }
                else
                {
                    min1 = Math.Min(endpoint11.Y, endpoint12.Y);
                    max1 = Math.Max(endpoint11.Y, endpoint12.Y);
                    min2 = Math.Min(endpoint21.Y, endpoint22.Y);
                    max2 = Math.Max(endpoint21.Y, endpoint22.Y);
                }
                min1 -= vertexAvgSize;
                max1 += vertexAvgSize;
                min2 -= vertexAvgSize;
                max2 += vertexAvgSize;
                if ((Math.Max(max1, max2) - Math.Min(min1, min2)) <= ((max1 - min1) + (max2 - min2)))
                {
                    return true;
                }
            }

            if ((endpoint11 - endpoint21).Length < maxDistanceForWallMerge ||
                (endpoint11 - endpoint22).Length < maxDistanceForWallMerge ||
                (endpoint12 - endpoint21).Length < maxDistanceForWallMerge ||
                (endpoint12 - endpoint22).Length < maxDistanceForWallMerge)
            {
                return true;
            }

            return false;
        }

        private static Vector3 ComputeNormal(IEnumerable<Vertex> neighbors, Vertex current)
        {
            // Compute normal for these N+1 points.
            List<Vector3> neighborPositionList = new List<Vector3>(numNeighbors + 1);
            foreach (var neighborVertex in neighbors)
            {
                neighborPositionList.Add(neighborVertex.Position);
            }
            if (current != null)
            {
                neighborPositionList.Add(current.Position);
            }

            var normal = CloudPointsHelper.ComputeEigenvectorOfMinEigenvalue(neighborPositionList);

            // As for geographical points, they should always face up (assumption)
            if (normal.Z < 0)
            {
                normal = -normal;
            }
            return normal;
        }

        private static Vector3 ComputeWallDirection(IEnumerable<Vertex> neighbors, Vector3 currentPosition, float vertexAvgSize)
        {
            float currentPositionZ = currentPosition.Z;

            List<Vector3> neighborPositionList = new List<Vector3>(numNeighbors + 1);
            var maxDeltaZ = vertexAvgSize * 3;
            foreach (var neighborVertex in neighbors)
            {
                if (Math.Abs(neighborVertex.Position.Z - currentPositionZ) < maxDeltaZ)
                {
                    neighborPositionList.Add(neighborVertex.Position);
                }
            }
            neighborPositionList.Add(currentPosition);

            var direction = CloudPointsHelper.ComputeEigenvectorOfMaxEigenvalue(neighborPositionList);
            return direction;
        }

        private static float ComputeMaxAngleBetweenNeighbors(Vertex vertex, List<Vertex> neighbors)
        {
            List<double> alphaList = new List<double>(neighbors.Count);
            for (int i = 0; i < neighbors.Count; i++)
            {
                Vector2 v = neighbors[i].Position.Xy - vertex.Position.Xy;
                double alpha;
                if (v.X > 0)
                {
                    alpha = Math.Atan(v.Y / v.X);
                }
                else if (v.X < 0)
                {
                    alpha = Math.Atan(v.Y / v.X) + Math.PI;
                }
                else
                {
                    if (v.Y > 0)
                    {
                        alpha = Math.PI / 2;
                    }
                    else if (v.Y < 0)
                    {
                        alpha = -Math.PI / 2;
                    }
                    else
                    {
                        continue;
                    }
                }
                alphaList.Add(alpha);
            }

            if (alphaList.Count <= 1)
            {
                return 0;
            }

            alphaList.Sort();

            double max = alphaList[0] - alphaList[alphaList.Count - 1] + Math.PI * 2;
            for (int i = 1; i < alphaList.Count; i++)
            {
                double alpha = alphaList[i] - alphaList[i - 1];
                if (alpha > max)
                {
                    max = alpha;
                }
            }

            return (float)max;
        }

        private static float ComputeMaxAngleBetweenNeighborsEx(Vertex vertex, List<Vertex> neighbors)
        {
            Vector3 N = Vector3.Normalize(vertex.Normal);
            Vector3 Ux = Vector3.UnitX - Vector3.Dot(N, Vector3.UnitX) * N;
            Vector3 Uy = Vector3.UnitY - Vector3.Dot(N, Vector3.UnitY) * N;

            List<float> alphaList = new List<float>(neighbors.Count);
            for (int i = 0; i < neighbors.Count; i++)
            {
                Vector3 V = neighbors[i].Position - vertex.Position;
                Vector3 U = V - Vector3.Dot(N, V) * N;
                float Ax = Vector3.CalculateAngle(U, Ux);
                float Ay = Vector3.CalculateAngle(U, Uy);

                float alpha = Ax;
                if (Ay > Math.PI / 2)
                {
                    alpha = -alpha;
                }
                alphaList.Add(alpha);
            }

            if (alphaList.Count <= 1)
            {
                return 0;
            }

            alphaList.Sort();

            float max = alphaList[0] - alphaList[alphaList.Count - 1] + (float)Math.PI * 2;
            for (int i = 1; i < alphaList.Count; i++)
            {
                float alpha = alphaList[i] - alphaList[i - 1];
                if (alpha > max)
                {
                    max = alpha;
                }
            }

            return max;
        }

        #endregion
    }
}
